#ifndef _C_POSE_3D_H_
#define _C_POSE_3D_H_

#include <iostream>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <sys/time.h>
#include <unistd.h>
#include <sys/select.h>

#include "SMATModel.hh"

#include "stereo.h"
#include "ransac.h"


class pose3D {
 public:
    CvMat *translation;
    CvMat * &T;
    CvMat *vect;
    CvMat * &R;
    CvMat *rotation;

 public:
 pose3D(void): T(translation), R(vect)
    {
	rotation = cvCreateMat( 3, 3, CV_32FC1 );
	translation = cvCreateMat( 3, 1, CV_32FC1 );
	vect = cvCreateMat(3,1, CV_32FC1);
    }
    
    pose3D ( pose3D &posein): T(translation), R(vect)
    {
	rotation = cvCreateMat(3,3,CV_32FC1);
	translation = cvCloneMat(posein.translation);
	vect = cvCloneMat(posein.vect);
    }
    
    ~pose3D()
    {
	cvReleaseMat(&rotation);
	cvReleaseMat(&vect);
	
	cvReleaseMat(&translation);
    }

    virtual pose3D & operator = (pose3D & pose);
    void resetR();
    void resetT();
    void rotate(float a, float b=0, float g=0);
    void translate(float x, float y=0, float z=0);
    void setrotate(float a, float b=0, float g=0);
    void settranslate(float x, float y=0, float z=0);
    void setTx(float x=0);
    void setTy(float y=0);
    void setTz(float z=0);
    void fprintf_T(FILE *f);
    void fprintf_R(FILE *f);
    void fprintf(FILE *f);
    void ChangeView(pose3D &posein, CALIBRATION_PARAMS_t *calibration); 
    float distanceT2(pose3D &pose2);
    float distanceR(pose3D &pose2);
    float Rx();
    float Ry();
    float Rz();
    float Tx();
    float Ty();
    float Tz();
};


struct line_t {
    int p1, p2;
    float z1, z2;
    CvScalar color;
    int thick;
};


class CView : public pose3D{
 public:
    CALIBRATION_PARAMS_t *calibration;
    CvMat *mask_show;
    
 public:
    
    CView(int points=0) 
    {
	mask_show =0;
	if (points)
	    alloc(points);
	calibration = 0;
    }

    CView(CView &view)
    {
	mask_show = cvCloneMat(view.mask_show);
	calibration = view.calibration;
    }

    ~CView()
    {
	if (mask_show)
	    cvReleaseMat(&mask_show);
	mask_show =0;
    }
    virtual void alloc(int points);
    virtual CView & operator = (CView & view);
    virtual void setmask_show(CvMat * maskin=NULL);
    virtual int deletepoint(int index);
    virtual void getNotHiden(CvMat * angY);
    void addpoint();
    void ChangeView(pose3D &posein); 

};

float distanceR(pose3D &pose1, pose3D &pose2);
float distanceT2(pose3D &pose1, pose3D &pose2);
float distance3x1_2(CvMat * pose1, CvMat  * pose2);


#endif
